/*
 Copyright 2013--Present JMM_PROGNAME
 
 This file is distributed under the terms of the JMM_PROGNAME License.
 
 You should have received a copy of the JMM_PROGNAME License.
 If not, see <JMM_PROGNAME WEBSITE>.
*/
// CREATED    : 7/15/2015
// LAST UPDATE: 10/1/2015

#include "statsxx/optimization/bracket_methods/Brent.hpp"

// STL
#include <cmath>      // std::sqrt()
#include <cstdlib>    // std::abs()
#include <functional> // std::function<>
#include <limits>     // std::numeric_limits<>
#include <tuple>      // std::tuple<>, std::make_tuple()

// jScience
#include "jScience/physics/consts.hpp" // golden_ratio2


inline Brent::Brent()
{
    this->tol = std::sqrt(std::numeric_limits<double>::epsilon());
    
    this->iter_max = 100.0;
}

inline Brent::~Brent() {};


//========================================================================
//========================================================================
//
// NAME: std::tuple<int,
//                  double,
//                  double> Brent::minimize(std::function<double(const double)> f,
//                                          double x1,
//                                          double x2)
//
// DESC: 
//
// INPUT:
//     std::function<double(const double)> f :
//     double x1 :
//     double x2 :
//
// OUTPUT:
//     int info :
//     double x_best; :
//     double f_best :
//
//========================================================================
//========================================================================
inline std::tuple<
                  int,
                  double,
                  double
                  > Brent::minimize(std::function<double(const double)> f,
                                    double x1,
                                    double x2)
{
    int info = 1;
    double x_best;
    double f_best;
    
    // first bracket the minimum (using the parent BracketMethod::bracket())
    double x3;
    double f1;
    double f2;
    double f3;
    std::tie(x1, x2, x3, f1, f2, f3) = this->bracket(f, x1, x2);
    
    // the minimum is always kept bracketed between x_low and x_high
    double x_low = x1;
    double x_high = x3;
    
    // x_best stores the minimum function found so far, x_best2 is the second best, etc.
    x_best = x2;
    double x_best2 = x2;
    double x_best3 = x2;
    
    f_best = f2;
    double f_best2 = f2;
    double f_best3 = f2;
    
    // initialize the steps moved, with prev_stp to 0 to force a golden section the on the first iteration
    double prev_stp = 0.0;
    double stp;
    
//    std::cout << "f_best: " << f_best << '\n';
    
    for(int iter = 0; iter < this->iter_max; ++iter)
    {
//        std::cout << "f_best(" << iter << "): " << f_best << '\n';
        
        double x_mid = (x_low + x_high)/2.0;
     
        double tol1 = this->tol*std::abs(x_best);
        double tol2 = 2.0*tol1;
        
        // convergence is achieved when x_best is close to x_mid, AND when x_high and x_low are close relative to tol2
        if( std::abs(x_best - x_mid) <= (tol2 - std::abs(x_high - x_low)/2.0) )
        {
            info = 0;
            
            break;
        }
        
        // if on the previous iteration we moved far enough, try a parabolic fit ...
        if( std::abs(prev_stp) > tol1 )
        {
            double tmp_stp = prev_stp;
            prev_stp = stp;
            
            double r = (x_best - x_best2)*(f_best - f_best3);
            double q = (x_best - x_best3)*(f_best - f_best2);
            
            double num = (x_best - x_best3)*q - (x_best - x_best2)*r;
            double denom = 2.0*(q - r);
            
            if(denom != 0.0)
            {
                stp = num/denom; // parabolic estimate
            }
            else
            {
                // if we can't make a parabolic estimake, use golden section
                
                if(x_best >= x_mid)
                {
                    prev_stp = x_low - x_best;
                }
                else
                {
                    prev_stp = x_high - x_best;
                }
                
                stp = golden_ratio2*prev_stp;
            }
            
            // as long as our step size is decreasing, and within the bounds ...
            if((std::abs(stp) < std::abs(tmp_stp/2.0)) &&
               ((x_best + stp) > x_low) &&
               ((x_best + stp) < x_high))
            {
                // ... we can use our parabolic estimate, BUT if we are close to the boundaries, then we must stabalize the search
                double xtmp = x_best + stp;

                if(((xtmp - x_low) < tol2) || ((x_high - xtmp) < tol2))
                {
                    if(x_best < x_mid)
                    {
                        stp = tol1;
                    }
                    else
                    {
                        stp = -tol1;
                    }
                }
            }
            // ... else the parabolic estimate must be poor, so advance by golden section
            else
            {
                if(x_best >= x_mid)
                {
                    prev_stp = x_low - x_best;
                }
                else
                {
                    prev_stp = x_high - x_best;
                }
                
                stp = golden_ratio2*prev_stp;
            }
        }
        // ... otherwise try a golden section search
        else
        {
            if(x_best >= x_mid)
            {
                prev_stp = x_low - x_best;
            }
            else
            {
                prev_stp = x_high - x_best;
            }
            
            stp = golden_ratio2*prev_stp;
        }
        
        // note that to justify such a trial, we must have moved a minimum distance
        double x_trial;
        
        if(std::abs(stp) >= tol1)
        {
            x_trial = x_best + stp;
        }
        else
        {
            if(stp > 0.0)
            {
                x_trial = x_best + tol1;
            }
            else
            {
                x_trial = x_best - tol1;
            }
        }
        
        // evaluate f() on our trial point
        double f_trial = f(x_trial);
        
        // if our trial point improved the solution ...
        if(f_trial <= f_best)
        {
            // ... shrink the boundaries 
            if(x_trial >= x_best)
            {
                x_low = x_best;
            }
            else
            {
                x_high = x_best;
            }
            
            // ... and update our storage
            x_best3 = x_best2;
            x_best2 = x_best;
            x_best = x_trial;
            
            f_best3 = f_best2;
            f_best2 = f_best;
            f_best = f_trial;
        }
        // ... else we did not improve
        else
        {
            // ... but we can still shrink the boundaries
            if(x_trial < x_best)
            {
                x_low = x_trial;
            }
            else
            {
                x_high = x_trial;
            }
            
            // ... and also check whether our trial is better than the stored second or third best values
            if((f_trial <= f_best2) || (x_best2 == x_best))
            {
                x_best3 = x_best2;
                x_best2 = x_trial;
                
                f_best3 = f_best2;
                f_best2 = f_trial;
            }
            else if((f_trial <= f_best3) || (x_best3 == x_best) || (x_best3 == x_best2))
            {
                x_best3 = x_trial;
                
                f_best3 = f_trial;
            }
        }
    }
    
    return std::make_tuple(info, x_best, f_best);
}

